/**
  ******************************************************************************
  * @file     
  * @author  Dantemiwa
  * @version 
  * @date    
  * @brief   
  ******************************************************************************
  * @attention
  *  
  *
  *
  * 
  ******************************************************************************
  */ 
/* Includes -------------------------------------------------------------------*/

#include "fort.h"
#include "stm32f4xx_usart.h"
#include "string.h"
#include "timer.h"
#include "math.h"
#include "includes.h"
#include "pps.h"
#include "usart.h"
#include "movebase.h"
//对应的收发串口
#define USARTX USART2
extern int temp1,temp2,temp3,temp4,temp5,temp6,temp7,temp8;
extern int getBallFlag,cycleTimes,crashTimes,mode,lastMode;
extern int x,y,speedX,speedY,angleZ,xShoot,yShoot;
extern int keyOneStatus,keyTwoStatus,keyThreeStatus,keyFourStatus,keyRedBlueStatus,keyWolkingStatus,keyCheckStatus;
extern float angle,testRps;
int scorerCnt=0,correctTimeCnt=0;   //分球器计数
char gestureSign='7';
float shooterVel=0,yawPos=0;
int yawFinishFlag=0,correctFlag=0,findBucketFlag=0,angleErr=0;
fort_t fort;
float err=0,integralAgl=0,yawAgl=0,lastYawAgl=0,overlayAgl=0;
float posData[3][50]={0};
char statusData[50]={0},colorData[50]={0};
int bufferI = 0;
char buffer[20] = {0};
extern int pinkColorFlag,usartCoordinateOne,usartCoordinateTwo,usartAngleThree,aimActualPosFlag,usartTimeCntColorTwo,usartTimeCntColorOne,usartStatus;
float leftDistance,rightDistance,detAngle,trueX,trueY,trueAngle;
extern int stopCollectRunFlag;
extern int giveBallTime;
/**
* @brief 炮台航向控制
* @param  ang:转台航向角度
* @retval none
* @attention none
*/
void YawPosCtrl(float ang)
{
		fort.usartTransmitData.dataFloat = ang;
		USART_SendData(USARTX,'Y');
		USART_SendData(USARTX,'A');
		USART_SendData(USARTX,fort.usartTransmitData.data8[0]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[1]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[2]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[3]);
		USART_SendData(USARTX,'\r');
		USART_SendData(USARTX,'\n');
		fort.usartTransmitData.data32 = 0;
		
}

/**
* @brief 发射电机转速控制
* @param  rps:发射电机速度，单位转每秒
* @retval none
* @attention none
*/
void ShooterVelCtrl(float rps)
{
		fort.usartTransmitData.dataFloat = rps;
		USART_SendData(USARTX,'S');
		USART_SendData(USARTX,'H');
		USART_SendData(USARTX,fort.usartTransmitData.data8[0]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[1]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[2]);
		USART_SendData(USARTX,fort.usartTransmitData.data8[3]);
		USART_SendData(USARTX,'\r');
		USART_SendData(USARTX,'\n');
		fort.usartTransmitData.data32 = 0;
}


void bufferInit()
{
	for(int i = 0; i < 20; i++)
	{
		buffer[i] = 0;
	}
	bufferI = 0;
}

/**
* @brief 接收炮台返回的数据
* @param data：串口每次中断接收到的一字节数据
* @retval none
* @attention 该函数请插入到对应的串口中断中
							注意清除标志位
*/
void GetValueFromFort(uint8_t data)
{
	buffer[bufferI] = data;
	bufferI++;
	if(bufferI >= 20)
	{
		bufferInit();
	}
	if(buffer[bufferI - 2] == '\r' && buffer[bufferI - 1] == '\n')
	{ 
		if(bufferI > 2 &&  strncmp(buffer,"PO",2) == 0)//接收航向位置  
		{
				for(int i = 0; i < 4; i++)
					fort.usartReceiveData.data8[i] = buffer[i + 2];
				fort.yawPosReceive = fort.usartReceiveData.dataFloat;
			  yawPos=fort.yawPosReceive;
		}
		else if(bufferI > 2 &&  strncmp(buffer,"VE",2) == 0)//接收发射电机转速
		{
				for(int i = 0; i < 4; i++)
					fort.usartReceiveData.data8[i] = buffer[i + 2];
				fort.shooterVelReceive = fort.usartReceiveData.dataFloat;
			  shooterVel=fort.shooterVelReceive;
		}
		else if(bufferI > 2 && strncmp(buffer,"VM",2) == 0)//接收摄像头的信息  
		{
				fort.cameraReceive.status = buffer[2]&0x0f;
				fort.cameraReceive.color=(buffer[2]&0xf0)>>4;
				for(int i = 0; i < 4; i++)
					fort.usartReceiveData.data8[i] = buffer[i + 3];
				fort.cameraReceive.disOne = fort.usartReceiveData.dataFloat;
				for(int i = 0; i < 4; i++)
					fort.usartReceiveData.data8[i] = buffer[i + 7];
				fort.cameraReceive.disTwo = fort.usartReceiveData.dataFloat;
				for(int i = 0; i < 4; i++)
					fort.usartReceiveData.data8[i] = buffer[i + 11];
				fort.cameraReceive.angle = fort.usartReceiveData.dataFloat;		
		}
		else if(bufferI > 2 && strncmp(buffer,"GS",2) == 0)//手势识别信息
		{
				fort.cameraReceive.gestureState = buffer[2];
		}
		bufferInit();
	}
}


/**
* @brief 对机器人进行坐标矫正
* @param  利用摄像头收集的数据  单位mm  单位  度
* @retval none
* @attention  此处应填上摄像头传递过来的数据
*/
extern float correctDetX,correctDetY,correctDetAgl;
extern int usartCorrectTimeCnt,usartCorrectFlag,usartCorrectTimes,correctStopAgl;
int dataI=0;
void Correct()
{
	static char finalStatus=10,finalColor=10,statusTmp;
	static int i,j,maxTmp=1,max=0,correctTimes=0,firstTurnRight=0,firstTurnLeft=0;
	static float coordinateOne,coordinateTwo,angleThree,coordinateOneTemp,coordinateTwoTemp,angleThreeTemp,trueX,trueY;
	correctTimeCnt++;
	if(correctTimes>9)   //矫正失败
	{
		dataI=0;
		findBucketFlag=0;    //找桶瞄准清零
		correctTimes=0;				//找桶次数
		correctFlag=0;				//矫正标志位
		correctTimeCnt=0;     //矫正计时
		mode=lastMode;
	}
	if(stopCollectRunFlag==0)   //??
	{
		aimActualPosFlag=0;
	}
	if(correctTimeCnt<240)      //转向四个角  2s                    
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		PidAngle(correctStopAgl);
		if(xShoot<0&&yShoot>2241.15f)  //左上角位置
		{ 
			yawAgl=overlayAgl+angle-90-atan((float)(yShoot-4641.15f)/(xShoot+2400.f))/PI*180; 						//瞄准（-2400,4641.15）
		}
		else if(xShoot<0&&yShoot<2241.15f)  //左下角位置
		{
			yawAgl=overlayAgl+angle-90-atan((float)(yShoot+159.85f)/(xShoot+2400.f))/PI*180;			//瞄准（-2400，-159.85）
		}
		else if(xShoot>0&&yShoot>2241.15f)   //右上角
		{
			yawAgl=overlayAgl+angle+90-atan((float)(yShoot-4641.15f)/(xShoot-2400.f))/PI*180;				//瞄准（2400，4641.15）
		}
		else
		{
			yawAgl=overlayAgl+angle+90-atan((float)(y+159.85f)/((x-2400.f)))/PI*180;		//瞄准（2400，-159.85）
		}
		if(firstTurnLeft==1)   //see left
		{
			if(findBucketFlag>1)   //瞄桶之后二次矫正失败
			{
				findBucketFlag=0;
				dataI=0;
				for(i=0;i<40;i++)
				{
					posData[0][i]=0;
					posData[0][i]=0;
					posData[0][i]=0;
					statusData[i]=0;
					colorData[i]=10;
				}
				correctTimeCnt=0;
				correctFlag=0;
			}
			if(correctTimes%2==1)
				yawAgl-=30*(1+correctTimes/2);
			else yawAgl+=30*(correctTimes/2);
		}
		else if(firstTurnRight==1)   //see right
		{
			if(findBucketFlag>1)
			{
				findBucketFlag=0;
				dataI=0;
				for(i=0;i<40;i++)
				{
					posData[0][i]=0;
					posData[0][i]=0;
					posData[0][i]=0;
					statusData[i]=0;
					colorData[i]=10;
				}
				correctTimeCnt=0;
				correctFlag=0;
			}
			if(correctTimes%2==1)
				yawAgl+=30*(1+correctTimes/2);
			else yawAgl-=30*(correctTimes/2);
		}
		if(yawAgl-yawPos>180)
		{
			yawAgl-=360;
			overlayAgl-=360;
		}
		if(yawAgl-yawPos<-180)
		{
			yawAgl+=360;
			overlayAgl+=360;
		}	
		YawPosCtrl(yawAgl);
		if(correctTimeCnt>70)
		{
			if(dataI==0&&fort.cameraReceive.status!=0)
			{
				statusData[dataI]=fort.cameraReceive.status;
				colorData[dataI]=fort.cameraReceive.color;
				posData[0][dataI]=fort.cameraReceive.disOne;
				posData[1][dataI]=fort.cameraReceive.disTwo;
				posData[2][dataI]=fort.cameraReceive.angle;
				dataI++;
			}
			else if(fort.cameraReceive.status!=0&&(fabs(posData[0][dataI-1]-fort.cameraReceive.disOne)>0.01||fabs(posData[0][dataI-1]-fort.cameraReceive.disOne)>0.01))
			{
				statusData[dataI]=fort.cameraReceive.status;
				colorData[dataI]=fort.cameraReceive.color;
				posData[0][dataI]=fort.cameraReceive.disOne;
				posData[1][dataI]=fort.cameraReceive.disTwo;
				posData[2][dataI]=fort.cameraReceive.angle;
				dataI++;
			}
		}
		if(firstTurnLeft==1||firstTurnRight==1)    //此刻正在摆头瞄
		{
			if(correctTimeCnt==130)
			{
				if(statusData[dataI]!=3&&statusData[dataI-1]!=3)   //这个角度没瞄到桶
				{
					dataI=0;
					for(i=0;i<40;i++)
					{
						posData[0][i]=0;
						posData[0][i]=0;
						posData[0][i]=0;
						statusData[i]=0;
						colorData[i]=10;
					}
					correctTimeCnt=0;
					correctTimes++;
				}
			}
		}
	}
	else if(correctTimeCnt==240)                                     //wait to get thr lastest data
	{
		for(i=0;i<dataI-1;i++)                //对n个数字排序，则需要进行n-1轮   冒泡排序法
		{
			for(j=0;j<dataI-1-i;j++)      //第一轮里边需要循环比较n-1次，确定最大的数，下一轮比较n-2次，确定次大的数，则j<n-1-i
			{
				if(posData[0][j]>posData[0][j+1])
				{
					coordinateOneTemp=posData[0][j],posData[0][j]=posData[0][j+1],posData[0][j+1]=coordinateOneTemp;
				}
				if(posData[1][j]>posData[1][j+1])
				{
					coordinateTwoTemp=posData[1][j],posData[1][j]=posData[1][j+1],posData[1][j+1]=coordinateTwoTemp;
				}
				if(posData[2][j]>posData[2][j+1])
				{
					angleThreeTemp=posData[2][j],posData[2][j]=posData[2][j+1],posData[2][j+1]=angleThreeTemp;
				}
				if(statusData[j]>statusData[j+1])
				{
					statusTmp=statusData[j],statusData[j]=statusData[j+1],statusData[j+1]=statusTmp;
				}
				if(colorData[j]>colorData[j+1])
				{
					statusTmp=colorData[j],colorData[j]=colorData[j+1],colorData[j+1]=statusTmp;
				}
			}
		}
		for(i=1;i<dataI;i++)
    {
			if(statusData[i]==statusData[i-1])
			{
				maxTmp+=1;
			}
			else if(statusData[i]!=statusData[i-1])
			{
				if(maxTmp>max)
				{
					finalStatus=statusData[i-1];
					max=maxTmp;
				}
				maxTmp=1;
			}
			if(i==dataI-1)
      {
				if(maxTmp>max)
				{
					finalStatus=statusData[i];
					max=maxTmp;
				}
				maxTmp=1;
				max=0;
			}
		}
		for(i=1;i<dataI;i++)
    {
			if(colorData[i]==colorData[i-1])
			{
				maxTmp+=1;
			}
			else if(colorData[i]!=colorData[i-1])
			{
				if(maxTmp>max)
				{
					finalColor=colorData[i-1];
					max=maxTmp;
				}
				maxTmp=1;
			}
			if(i==dataI-1)
      {
				if(maxTmp>max)
				{
					finalColor=colorData[i];
					max=maxTmp;
				}
				maxTmp=1;
				max=0;
			}
		}
		coordinateOne=(posData[0][dataI/2-6]+posData[0][dataI/2-5]+posData[0][dataI/2-4]+posData[0][dataI/2-3]+posData[0][dataI/2-2]+posData[0][dataI/2-1]+posData[0][dataI/2]+posData[0][dataI/2+1]+posData[0][dataI/2+2]+posData[0][dataI/2+3]+posData[0][dataI/2+4]+posData[0][dataI/2+5]+posData[0][dataI/2+6])/13.f;
		coordinateTwo=(posData[1][dataI/2-6]+posData[1][dataI/2-5]+posData[1][dataI/2-4]+posData[1][dataI/2-3]+posData[1][dataI/2-2]+posData[1][dataI/2-1]+posData[1][dataI/2]+posData[1][dataI/2+1]+posData[1][dataI/2+2]+posData[1][dataI/2+3]+posData[1][dataI/2+4]+posData[1][dataI/2+5]+posData[1][dataI/2+6])/13.f;
		angleThree=(posData[2][dataI/2-6]+posData[2][dataI/2-5]+posData[2][dataI/2-4]+posData[2][dataI/2-3]+posData[2][dataI/2-2]+posData[2][dataI/2-1]+posData[2][dataI/2]+posData[2][dataI/2+1]+posData[2][dataI/2+2]+posData[2][dataI/2+3]+posData[2][dataI/2+4]+posData[2][dataI/2+5]+posData[2][dataI/2+6])/13.f; 
		usartCoordinateOne=coordinateOne;
		usartCoordinateTwo=coordinateTwo;
		usartAngleThree=angleThree;
		usartStatus=finalStatus;
		for(i=0;i<dataI;i++)
		{
			USART_OUT(USART1,"\t%d\t",(int)posData[0][i]);
		}
		USART_OUT(USART1,"\r\n");
		for(i=0;i<dataI;i++)
		{
			USART_OUT(USART1,"\t%d\t",(int)posData[1][i]);
		}
		USART_OUT(USART1,"\r\n");
		for(i=0;i<dataI;i++)
		{
			USART_OUT(USART1,"\t%d\t",(int)(posData[2][i]*1000));
		}
		USART_OUT(USART1,"\r\n");
		for(i=0;i<dataI;i++)
		{
			USART_OUT(USART1,"\t%d\t",(int)statusData[i]);
		}
		USART_OUT(USART1,"\r\n");
		for(i=0;i<dataI;i++)
		{
			USART_OUT(USART1,"\t%d\t",(int)colorData[i]);
		}
		USART_OUT(USART1,"\r\n");
		float trueAlpha;
		trueAlpha=yawAgl-overlayAgl;  //航向电机转的角度180--180
		trueAlpha=angle-trueAlpha+90;           //摄像头偏离y轴正方向的角度，左正右负
		while(trueAlpha>180) trueAlpha-=360;
		while(trueAlpha<-180) trueAlpha+=360;
		if(finalStatus==3)
		{
			if(keyRedBlueStatus==0)        //选择蓝场
			{
				trueX=coordinateOne+20*sin(trueAlpha)	;  //炮台中心为原点的炮台的坐标   //20为炮台到航向的距离
				trueY=coordinateTwo-20*cos(trueAlpha)+2241.15f;
			}
			else if(keyRedBlueStatus==1)
			{
				trueX=-coordinateOne+20*sin(trueAlpha);
				trueY=-coordinateTwo-20*cos(trueAlpha)+2241.15f;
			}
			static float temp1,temp2,temp3,temp4;
			temp1=90+atan((trueY-4641.15f)/(trueX+2400.f))/PI*180+angleThree+yawAgl-overlayAgl;
			temp2=90+atan((trueY+159.85f)/(trueX+2400.f))/PI*180+angleThree+yawAgl-overlayAgl;
			temp3=-90+atan((trueY-4641.15f)/(trueX-2400.f))/PI*180+angleThree+yawAgl-overlayAgl;
			temp4=-90+atan((trueY+159.85f)/(trueX-2400.f))/PI*180+angleThree+yawAgl-overlayAgl;	
//			if(abs(xShoot)>400&&abs(xShoot)<1900&&fabs(yShoot-2241.15f)>400&&fabs(yShoot-2241.15f)<1900)   //合适的位置才会矫正角度
//			{
				if((keyRedBlueStatus==0&&finalColor==3)||(keyRedBlueStatus==1&&finalColor==4))  //左上角位置
				{
	//				if(fabs(temp1-angle)>2&&fabs(temp1-angle)<15)
	//				{
						correctDetAgl=temp1-GetAngle();
						angle=temp1;
	//				}
				}
				else if((keyRedBlueStatus==0&&finalColor==1)||(keyRedBlueStatus==1&&finalColor==2))  //左下角位置
				{
	//				if(fabs(temp2-angle)>2&&fabs(temp2-angle)<15)
	//				{
						correctDetAgl=temp2-GetAngle();
						angle=temp2;
	//				}
				}
				else if((keyRedBlueStatus==0&&finalColor==2)||(keyRedBlueStatus==1&&finalColor==1))   //右上角
				{
	//				if(fabs(temp3-angle)>2&&fabs(temp3-angle)<15)
	//				{
						correctDetAgl=temp3-GetAngle();
						angle=temp3;
	//				}
				}
				else if((keyRedBlueStatus==0&&finalColor==4)||(keyRedBlueStatus==1&&finalColor==3))  //右下角
				{
	//				if(fabs(temp4-angle)>2&&fabs(temp4-angle)<15)
	//				{
						correctDetAgl=temp4-GetAngle();
						angle=temp4;
	//				}
				}
//			}
			trueX=trueX-86.85f*sin(angle*PI/180.f);  //定位系统为原点的定位系统的坐标
			trueY=trueY-86.85f+86.85f*cos(angle*PI/180.f);
//			if(fabs(trueX-GetX()-correctDetX)>20&&fabs(trueX-GetX()-correctDetX)<1000)
//			{
				correctDetX=trueX-GetX();
//			}
//			if(fabs(trueY-GetY()-correctDetY)>20&&fabs(trueY-GetY()-correctDetY)<1000)
//			{
				correctDetY=trueY-GetY();
//			}
			x=(trueX+170.7f*sin((angle*PI/180.f)));     //车轴为原点，车轴的坐标
			y=(trueY+170.7f*(1-cos((angle*PI/180.f))));
			correctTimeCnt=0;
			correctTimes=0;
			if(firstTurnRight==1||firstTurnLeft==1)
			{
				findBucketFlag++;
				firstTurnRight=0;
				firstTurnLeft=0;
			}
			else correctFlag=0;
			if(fabs(angleThree)>13)
			{
				angleErr++;
				correctFlag=1;
			}
			else angleErr=0;
			if(angleErr>1)
			{
				angleErr=0;
				correctFlag=0;
			}
			finalStatus=10;
			USART_OUT(USART1,"ppsX\t%d\t",(int)trueX);        //坐标矫正量
			USART_OUT(USART1,"ppsY\t%d\t",(int)trueY);
			USART_OUT(USART1,"dtX\t%d\t",(int)correctDetX);        //坐标矫正量
			USART_OUT(USART1,"dtY\t%d\t",(int)correctDetY);	
			USART_OUT(USART1,"dtA\t%d\t",(int)(correctDetAgl*100));	
			USART_OUT(USART1,"one\t%d\t",(int)usartCoordinateOne);       
			USART_OUT(USART1,"two\t%d\t",(int)usartCoordinateTwo);	
			USART_OUT(USART1,"thr\t%d\t", (int)(angleThree*100));
			USART_OUT(USART1,"X\t%d\t",x);
			USART_OUT(USART1,"Y\t%d\r\n",y);
		}
		else if(finalStatus==1)// 只看见一边
		{
			correctTimes++;
			findBucketFlag++;    //找桶瞄准
			if(firstTurnRight==1||firstTurnLeft==1)
			{
				//无操作
			}
			else if(angleThree>0)
				firstTurnRight=1;
			else if(angleThree<0)
				firstTurnLeft=1;
			correctTimeCnt=0;
			finalStatus=0;
		}
		else  if(finalStatus==0) //nothing to see but the PC is working
		{
			correctTimes++;
			if(firstTurnRight==1||firstTurnLeft==1)
			{
				correctTimeCnt=0;
			}
			else
			{
				findBucketFlag++;    //找桶瞄准
				firstTurnRight=1;
				correctTimeCnt=0;
			}
			finalStatus=0;
		}
		else//ststus为4或其他,比如断开，不矫正
		{
			findBucketFlag=0;    //找桶瞄准清零
			correctTimes=0;
			mode=lastMode;
			correctTimeCnt=0;
			correctFlag=0;
			finalStatus=0;
		}
		dataI=0;
		for(i=0;i<40;i++)
		{
			posData[0][i]=0;
			posData[0][i]=0;
			posData[0][i]=0;
			statusData[i]=10;
			colorData[i]=10;
		}
	}
	else
	{
		correctTimes=0;
		dataI=0;
		for(i=0;i<40;i++)
		{
			posData[0][i]=0;
			posData[0][i]=0;
			posData[0][i]=0;
			statusData[i]=0;
			colorData[i]=10;
		}
		correctTimeCnt=0;
		correctFlag=0;
		finalStatus=0;
		findBucketFlag=0;    //找桶瞄准清零
	}
	usartCorrectTimeCnt=correctTimeCnt;
	usartCorrectTimes=correctTimes;
}

/**
* @brief 对球进行颜色识别，并且瞄准设计
* @param  利用摄像头收集的数据 
* @retval none
* @attention  none
*/
extern int32_t actual1Vel,actual2Vel,actual7Pos;	
extern int mode;
extern char ballColor[]; //0 无，1白，2黑，3粉
int turnBallFlag=0,lastMotorActualPos=0;
float shootRaps=30;
extern int xShoot,yShoot;
int aimActualPosFlag=0;
int motorActualPos=0;
int actV;
float rapsV,angleTan;
float shootDistance;
int colorChangeFlag=0;
extern int checktemp1,checktemp2,checktemp3;
void collectBall(void)
{
	if(motorActualPos!=2)
	{
		lastMotorActualPos=motorActualPos;
	}
	ReadActualPos(CAN2,7);
	static int timeCntOne=0,timeCntTwo=0;
	//第一收球位置       
	static int ballOneCnt1=0,ballTwoCnt1=0,ballThreeCnt1=0;
	if(motorActualPos==0)
	{
		static int timeCntColorOne=0;
		static int turnBackFlagOne=0;
		usartTimeCntColorOne=timeCntColorOne;
		if(stopCollectRunFlag==1)
		{
			stopCollectRunFlag=0;
			timeCntColorOne=32;
		}
		if(mode!=5)
		{
			timeCntColorOne=0;
			turnBackFlagOne=0;
		}
		if(ballColor[0]==1||ballColor[0]==2||ballColor[0]==3||turnBackFlagOne==1)
		{
			timeCntColorOne++;
			timeCntTwo=0;
			if(ballColor[0]==1)
			{
				ballOneCnt1++;
			}
			if(ballColor[0]==2)
			{
				ballTwoCnt1++;
			}
			if(ballColor[0]==3)
			{
				ballThreeCnt1++;
			}
			if(timeCntColorOne>15)
			{
				timeCntOne=0;
			}
			if(timeCntColorOne>25||turnBackFlagOne==1)
			{
				if(timeCntColorOne==26)
				{
					if((ballOneCnt1>=ballTwoCnt1)&&(ballOneCnt1>=ballThreeCnt1)) aimActualPosFlag=1;					
					if((ballTwoCnt1>=ballOneCnt1)&&(ballTwoCnt1>=ballThreeCnt1)) aimActualPosFlag=2;
					if((ballThreeCnt1>=ballTwoCnt1)&&(ballThreeCnt1>=ballOneCnt1))
					{
						if(colorChangeFlag)
						{
							aimActualPosFlag=3;
							colorChangeFlag=!colorChangeFlag;
						}
						else
						{
							aimActualPosFlag=4;
							colorChangeFlag=!colorChangeFlag;
						}
					}
					turnBackFlagOne=1;
				}
				ballOneCnt1=0;
				ballTwoCnt1=0;
				ballThreeCnt1=0;
//				if(timeCntColorOne<90&&timeCntColorOne>61)
//				{
//					PosCrl(CAN2,7,ABSOLUTE_MODE,8000);
//				}
				if((timeCntColorOne>25&&yawFinishFlag==1&&fabs(shooterVel-shootRaps)<0.8f&&sqrt(pow(speedX,2)+pow(speedY,2))<80)||(timeCntColorOne>300))
				{
					static int finishRapsTimeCnt=0;
					if(finishRapsTimeCnt++>10)
					{
						PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
						if(aimActualPosFlag==3||aimActualPosFlag==4)
						{
							pinkColorFlag=1;
						}
						motorActualPos=2;
						timeCntColorOne=0;
						turnBackFlagOne=0;
						finishRapsTimeCnt=0;
						yawFinishFlag=0;
					}
				}
			}
			else
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,0);
			}
		}
		else
		{
			timeCntOne++;
			if(timeCntOne==80)
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
				motorActualPos=2;
			}
		}
	}
//第二收球位置
	static int ballOneCnt=0,ballTwoCnt=0,ballThreeCnt=0;
	if(motorActualPos==1)
	{
		static int timeCntColorTwo=0;
		static int turnBackFlagTwo=0;
		usartTimeCntColorTwo=timeCntColorTwo;
		if(stopCollectRunFlag==1)
		{
			stopCollectRunFlag=0;
			timeCntColorTwo=32;
		}
		if(mode!=5)
		{
			timeCntColorTwo=0;
			turnBackFlagTwo=0;
		}
		if(ballColor[1]==1||ballColor[1]==2||ballColor[1]==3)
		{
			timeCntColorTwo++;
			timeCntOne=0;
			if(ballColor[1]==1)
			{
				ballOneCnt++;
			}
			if(ballColor[1]==2)
			{
				ballTwoCnt++;
			}
			if(ballColor[1]==3)
			{
				ballThreeCnt++;
			}
			if(timeCntColorTwo>15)
			{
				timeCntTwo=0;
			}
			if(timeCntColorTwo>25||turnBackFlagTwo==1)
			{
				if(timeCntColorTwo==26)
				{
						if((ballOneCnt>=ballTwoCnt)&&(ballOneCnt>=ballThreeCnt))
						{
							aimActualPosFlag=1;
							checktemp2=1;
						}
						if((ballTwoCnt>=ballOneCnt)&&(ballTwoCnt>=ballThreeCnt))
						{
							aimActualPosFlag=2;
							checktemp2=2;
						}
						if((ballThreeCnt>=ballTwoCnt)&&(ballThreeCnt>=ballOneCnt))
						{
							checktemp2=3;
							if(colorChangeFlag)
							{
								aimActualPosFlag=3;
								colorChangeFlag=!colorChangeFlag;
							}
							else
							{
								aimActualPosFlag=4;
								colorChangeFlag=!colorChangeFlag;
							}	
						}
						turnBackFlagTwo=1;
				}
				ballOneCnt=0;
				ballTwoCnt=0;
				ballThreeCnt=0;
				if((timeCntColorTwo>25&&yawFinishFlag==1&&fabs(shooterVel-shootRaps)<0.8f&&sqrt(pow(speedX,2)+pow(speedY,2))<80)||(timeCntColorTwo>300))
				{
					static int finishRapsTimeCnt=0;
					if(finishRapsTimeCnt++>10)
					{
						PosCrl(CAN2,7,ABSOLUTE_MODE,0);
						if(aimActualPosFlag==3||aimActualPosFlag==4)
						{
							pinkColorFlag=1;
						}
						motorActualPos=2;
						timeCntColorTwo=0;
						turnBackFlagTwo=0;
						yawFinishFlag=0;
						finishRapsTimeCnt=0;
					}
					checktemp3=finishRapsTimeCnt;
				}
			}
			else
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
			}
		}
	else
		{
			timeCntTwo++;
			if(timeCntTwo==80)
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,0);
				motorActualPos=2;
				timeCntColorTwo=0;
			}
		}
	}
	//中间状态位置  
	if(motorActualPos==2)
	{
		static int lastActual7Pos=0;
		static int checkTimeCnt=0;
		static int checkFlag=0;
		static int PosTimeCnt=0;
		PosTimeCnt++;
		PosTimeCnt=PosTimeCnt%10;
		if(PosTimeCnt==9)
		{
			if(abs(lastActual7Pos-actual7Pos)<400)checkTimeCnt++;
			lastActual7Pos=actual7Pos;
		}
		if(checkTimeCnt>5)
		{
			if(lastMotorActualPos==0)
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,0);
				checkTimeCnt=0;
				checkFlag=1;
			}
			else if(lastMotorActualPos==1)
			{
				PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
				checkFlag=2;
				checkTimeCnt=0;
			}
		}
		static int timeCntChange=0;
		timeCntOne=0;
		timeCntTwo=0;
		ballOneCnt1=0;
		ballTwoCnt1=0;
		ballThreeCnt1=0;
		ballOneCnt=0;
		ballTwoCnt=0;
		ballThreeCnt=0;
		if(lastMotorActualPos==1)
		{
			if(actual7Pos>-600&&actual7Pos<600/*&&checkFlag!=1&&checkFlag!=2*/)
			{
				timeCntChange++;
				if(timeCntChange>25)
				{
					motorActualPos=0;
					checkTimeCnt=0;
					timeCntChange=0;
				}
			}
		}
		if(lastMotorActualPos==0)
		{
			if(actual7Pos<-27668&&actual7Pos>-28668/*&&checkFlag!=1&&checkFlag!=2*/)
			{
				timeCntChange++;
				if(timeCntChange>25)
				{
					motorActualPos=1;
					timeCntChange=0;
					checkTimeCnt=0;
				}
			}
		}
		switch(checkFlag)
		{ 
			static int checkTimeZero=0;
			static int checkTimeOne=0;
			case 0:
				break;
			case 1:
				if(checkTimeZero++>100)
				{
					checkTimeZero=0;
					PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
					checkFlag=0;
				}
				checkTimeCnt=0;
				break;
			case 2:
				if(checkTimeOne++>100)
				{
					checkTimeOne=0;
					PosCrl(CAN2,7,ABSOLUTE_MODE,0);
					checkFlag=0;
				}
				checkTimeCnt=0;
				break;
		}
	}
}
void collectBallRun(void)  //定点打球运动过程中的分球器响应   
{
	if(stopCollectRunFlag==0)
	{
		ReadActualPos(CAN2, 7);
		static int timeCntLeft=0,timeCntRight=0;
		//第一收球位置       
		static int ballWhiteCnt=0,ballBlackCnt=0,ballPinkCnt=0;
		if(motorActualPos==0)
		{
			static int timeCntColorOne=0;
			if(ballColor[0]==1||ballColor[0]==2||ballColor[0]==3)
			{
				timeCntColorOne++;       //计识别到球的时间
				timeCntRight=0;
				if(ballColor[0]==1)
				{
					ballWhiteCnt++;
				}
				if(ballColor[0]==2)
				{
					ballBlackCnt++;
				}
				if(ballColor[0]==3)
				{
					ballPinkCnt++;
				}
				if(timeCntColorOne>35)
				{
					timeCntLeft=0;
				}
				if(timeCntColorOne>40)
				{
					if(timeCntColorOne==41)
					{
						if((ballWhiteCnt>=ballBlackCnt)&&(ballWhiteCnt>=ballPinkCnt))
						{
							aimActualPosFlag=1;	              //白球
						}
						if((ballBlackCnt>=ballWhiteCnt)&&(ballBlackCnt>=ballPinkCnt))
						{
							aimActualPosFlag=2;  //黑球
						}
						if((ballPinkCnt>=ballBlackCnt)&&(ballPinkCnt>=ballWhiteCnt))
						{
							aimActualPosFlag=5;   //粉球
						}
					}
					ballWhiteCnt=0;
					ballBlackCnt=0;
					ballPinkCnt=0;
					if(keyRedBlueStatus==0)  //选择蓝场
					{
						if(aimActualPosFlag==1)   //非得分球
						{
							PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
							giveBallTime++;
							motorActualPos=2;
							timeCntColorOne=0;
							yawFinishFlag=0;
							if(cycleTimes==0)
							{
								getBallFlag=1;
							}
						}
						else 				
						{
							PosCrl(CAN2,7,ABSOLUTE_MODE,0);
							getBallFlag=1;
							timeCntColorOne=0;
							yawFinishFlag=0;
						}
					}
					else //选择红场
					{
						if(aimActualPosFlag==2)   //非得分球
						{
							if(cycleTimes==0)
							{
								getBallFlag=1;
							}
							PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
							motorActualPos=2;
							giveBallTime++;
							timeCntColorOne=0;
							yawFinishFlag=0;
						}
						else 
						{
							PosCrl(CAN2,7,ABSOLUTE_MODE,0);
							getBallFlag=1;
							timeCntColorOne=0;
							yawFinishFlag=0;
						}
					}
				}
				else
				{
					PosCrl(CAN2,7,ABSOLUTE_MODE,0);
				}
			}
			else
			{
				timeCntLeft++;
				if(timeCntLeft==50)
				{
					PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
					motorActualPos=2;
				}
			}
		}
	//第二收球位置
		static int ballOneCnt=0,ballTwoCnt=0,ballThreeCnt=0;
		if(motorActualPos==1)
		{
			static int timeCntColorTwo=0;
			if(ballColor[1]==1||ballColor[1]==2||ballColor[1]==3)
			{
				timeCntColorTwo++;
				timeCntLeft=0;
				if(ballColor[1]==1)
				{
					ballOneCnt++;
				}
				if(ballColor[1]==2)
				{
					ballTwoCnt++;
				}
				if(ballColor[1]==3)
				{
					ballThreeCnt++;
				}
				if(timeCntColorTwo>35)
				{
					timeCntRight=0;
				}
				if(timeCntColorTwo>40)
				{
					if(timeCntColorTwo==41)
					{
						if((ballOneCnt>=ballTwoCnt)&&(ballOneCnt>=ballThreeCnt))
						{
							aimActualPosFlag=1;
						}
						if((ballTwoCnt>=ballOneCnt)&&(ballTwoCnt>=ballThreeCnt))
						{
							aimActualPosFlag=2;
						}
						if((ballThreeCnt>=ballTwoCnt)&&(ballThreeCnt>=ballOneCnt))
						{
							aimActualPosFlag=5;
						}
					}
					ballOneCnt=0;
					ballTwoCnt=0;
					ballThreeCnt=0;
					if(keyRedBlueStatus==0)  //选择蓝场
					{
						if(aimActualPosFlag==1)
						{
							if(cycleTimes==0)
							{
								getBallFlag=1;
							}
							PosCrl(CAN2,7,ABSOLUTE_MODE,0);
							giveBallTime++;
							motorActualPos=2;
							timeCntColorTwo=0;
							yawFinishFlag=0;
						}
						else
						{
							PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
							getBallFlag=1;
							timeCntColorTwo=0;
							yawFinishFlag=0;
						}
					}
					else 
					{
						if(aimActualPosFlag==2)
						{
							if(cycleTimes==0)
							{
								getBallFlag=1;
							}
							PosCrl(CAN2,7,ABSOLUTE_MODE,0);
							giveBallTime++;
							motorActualPos=2;
							timeCntColorTwo=0;
							yawFinishFlag=0;
						}
						else
						{
							PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
							getBallFlag=1;
							timeCntColorTwo=0;
							yawFinishFlag=0;
						}
					}
				}
				else
				{
					PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
				}
			}
			else
			{
				timeCntRight++;
				if(timeCntRight==50)
				{
					PosCrl(CAN2,7,ABSOLUTE_MODE,0);
					motorActualPos=2;
					timeCntColorTwo=0;
				}
			}
		}
	//中间状态位置  
		if(motorActualPos==2)
		{
			static int timeCntChange=0;
			timeCntLeft=0;
			timeCntRight=0;
			ballWhiteCnt=0;
			ballBlackCnt=0;
			ballPinkCnt=0;
			ballOneCnt=0;
			ballTwoCnt=0;
			ballThreeCnt=0;
			if(actual7Pos>-800&&actual7Pos<800)
			{
				timeCntChange++;
				if(timeCntChange>25)
				{
					motorActualPos=0;
					timeCntChange=0;
				}
			}
			if(actual7Pos<-27368&&actual7Pos>-28968)
			{
				timeCntChange++;
				if(timeCntChange>25)
				{
					motorActualPos=1;
					timeCntChange=0;
				}
			}
		}
	}
	else getBallFlag=1;
}

/**
* @brief 瞄准函数系列
* @param   
* @retval none
* @attention  none
*/
void CenterAim(void)
{
	if(xShoot==0)                                        //特殊点  
	{
		if(yShoot<2241.15)	
			yawAgl=angle+overlayAgl;
		else 
			yawAgl=angle+180.f+overlayAgl;
	}
	else                                           //一般情况
	{
		if(xShoot>0)
		{
			yawAgl=angle-90-atan((float)(yShoot-2241.15)/(xShoot*1.0))/PI*180+overlayAgl;
		}
		else if(xShoot<0)
		{
			yawAgl=angle+90-atan((float)(yShoot-2241.15)/(xShoot*1.0))/PI*180+overlayAgl;				
		}
	}
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void BlueOne(void)
{
	if(xShoot>=2200)
		yawAgl=angle+180+overlayAgl;
	else
		yawAgl=angle+90-atan((float)(yShoot-41.15)/((xShoot-2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void BlueTwo(void)
{
	if(xShoot<=-2200)
		yawAgl=angle-180+overlayAgl;
	else
		yawAgl=angle-90-atan((float)(yShoot-41.15)/((xShoot+2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);	
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;	
}
void BlueThree(void)
{	
	if(xShoot<=-2200) yawAgl=angle+overlayAgl;
	else yawAgl=angle-90-atan((float)(yShoot-4441.15)/((xShoot+2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;	
}	
void BlueFour(void)
{
	if(xShoot>=2200) yawAgl=angle+overlayAgl;
	else yawAgl=angle+90-atan((float)(yShoot-4441.15)/((xShoot-2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void RedOne(void)
{
	if(xShoot<=-2200)	yawAgl=angle+overlayAgl;
	else yawAgl=angle-90-atan((float)(yShoot-4441.15)/((xShoot+2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);		
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void RedTwo(void)
{
	if(xShoot>=2200)yawAgl=angle+overlayAgl;
	else yawAgl=angle+90-atan((float)(yShoot-4441.15)/((xShoot-2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);	
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void RedThree(void)
{
	if(xShoot>=2200) yawAgl=angle-180+overlayAgl;
	else yawAgl=angle+90-atan((float)(yShoot-41.15)/((xShoot-2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}
void RedFour(void)
{	
	if(xShoot<=-2200) yawAgl=angle-180+overlayAgl;
	else yawAgl=angle-90-atan((float)(yShoot-41.15)/((xShoot+2200)*1.0))/PI*180+overlayAgl;
	if(yawAgl-lastYawAgl>180)
	{
		yawAgl-=360;
		overlayAgl-=360;
	}
	else if(yawAgl-lastYawAgl<-180)
	{
		yawAgl+=360;
		overlayAgl+=360;
	}
	YawPosCtrl(yawAgl);	
	if(fabs(yawPos-yawAgl)<2)
	{
		yawFinishFlag=1;
	}
	else yawFinishFlag=0;
	lastYawAgl=yawAgl;
}

/**
* @brief 不要视觉的坐标矫正
* @param   
* @retval none
* @attention  none
*/
void BlindCorrect(void)
{
	
}	

/**
* @brief 清空多余球球函数
* @param   
* @retval none
* @attention  none
*/
void ClearBall(void)
{
	ReadActualPos(CAN2,7); 
	scorerCnt++;
	scorerCnt=scorerCnt%300;
	if(scorerCnt<150)
	{
		PosCrl(CAN2,7,ABSOLUTE_MODE,0);
	}

	else
	 PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
//	YawPosCtrl(-2.5);
//	ShooterVelCtrl(testRps);
}

/**
* @brief 手势识别
* @param   
* @retval none
* @attention  none
*/

void GestureRecognition(void)
{
	gestureSign=fort.cameraReceive.gestureState;
	if(gestureSign =='0')
	{
		
	}
	else if(gestureSign=='1')
	{
		
	}
	else if(gestureSign=='2')
	{
		
	}
	else if(gestureSign=='3')
	{
		
	}
	else if(gestureSign=='4')
	{
		
	}
	else if(gestureSign=='5')
	{
		
	}
	else if(gestureSign=='6')
	{
		
	}
	else 
	{
		
	}
}
/*
	判定球仓是几号球仓
	1：选择1、2
	2：选择1、3
	3：选择1、4
	4：选择2、3
	5：选择2、4
	6：选择3、4
*/
int backBallStore(int caseWhat)
{
	if(keyOneStatus==1&&keyTwoStatus==1)
	{
		caseWhat=1;
	}
	else if(keyOneStatus==1&&keyThreeStatus==1)
	{
		caseWhat=2;
	}
	else if(keyOneStatus==1&&keyFourStatus==1)
	{
		caseWhat=3;
	}
	else if(keyTwoStatus==1&&keyThreeStatus==1)
	{
		caseWhat=4;
	}
	else if(keyTwoStatus==1&&keyFourStatus==1)
	{
		caseWhat=5;
	}
	else
	{
		caseWhat=6;
	}
	return caseWhat;
}
